#ifndef ARUCO_LOCATION_H 
#define ARUCO_LOCATION_H 

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>

#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

#include "aruco_location/read_param.hpp"

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>

#include <geometry_msgs/PoseArray.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Transform.h>

void euler2Quat(const Eigen::Vector3d &eu,  Eigen::Quaterniond &q)
{
    q = Eigen::Quaterniond(Eigen::AngleAxisd(eu[2], Eigen::Vector3d::UnitZ()) *
	Eigen::AngleAxisd(eu[1], Eigen::Vector3d::UnitY()) *
	Eigen::AngleAxisd(eu[0], Eigen::Vector3d::UnitX()));
}

namespace aruco_location
{

class ResultPose
{
    public:
        cv::Vec3d rvec, tvec;
        std::vector<cv::Vec3d> rvecs, tvecs;
};

class ArucoLocation
{
 public:
    ArucoLocation(const std::string &config_path);
    ~ArucoLocation(){};
    void setParamters();
    bool detectAruco();

    void callback(const sensor_msgs::ImageConstPtr &msg);

 private:
    std::string config_path_;

    double aruco_len_;
    int aruco_format_id_;
    cv::Mat cv_K_, cv_Dist_;

    cv::Mat img_;
    // 根据序号,初始化dictionary参数指示板标记属于哪个标记字典。
    cv::Ptr<cv::aruco::Dictionary> dict_; // 编码词典 
    
    int aruco_index_;
    std::vector<int> markerIds_; // id序号  储存检测的marker对应的id号
    std::vector< std::vector<cv::Point2f> > marker_corners_; // 检测成功的角点信息4个点
    std::unique_ptr<ResultPose> result_pose_;  // 检测成功的位姿
    
    bool is_draw_pose_;
    cv::Mat draw_img_;
    ros::NodeHandle n_;
    ros::Subscriber sub_img_;
    ros::Publisher  pub_img_, pub_pose_array_, pub_pose_;
    std::string topic_detect_, topic_img_, topic_pose_;

    geometry_msgs::PoseArrayPtr pose_array_;
    geometry_msgs::PoseStamped pose_;
    tf2_ros::TransformBroadcaster broadcaster_;

    Eigen::Quaterniond qwc_;
    Eigen::Vector3d twc_;

    ros::Time msgStamp_;

    std::string world_frame_id_, camera_frame_id_, aruco_frame_id_;
    int pub_TF_flag_;
    int pub_image_flage_;
};

}

#endif